// --------------------------------------------------------------------------------------------------------------------
// <copyright file="HomogenousTransformTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;

    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    using ShoNS.Array;

    /// <summary>
    /// Test the HomogenousTransform class
    /// </summary>
    [TestClass]
    public class HomogenousTransformTest
    {
        /// <summary>
        /// Unit vector in positive X direction
        /// </summary>
        private readonly Vector3 unitPosX = new Vector3(1.0, 0, 0);

        /// <summary>
        /// Unit vector in positive Y direction
        /// </summary>
        private readonly Vector3 unitPosY = new Vector3(0, 1.0, 0);

        /// <summary>
        /// Unit vector in positive Z direction
        /// </summary>
        private readonly Vector3 unitPosZ = new Vector3(0, 0, 1.0);

        /// <summary>
        /// Unit vector in negative X direction
        /// </summary>
        private readonly Vector3 unitNegX = new Vector3(-1.0, 0, 0);

        /// <summary>
        /// Unit vector in negative Y direction
        /// </summary>
        private readonly Vector3 unitNegY = new Vector3(0, -1.0, 0);

        /// <summary>
        /// Unit vector in negative Z direction
        /// </summary>
        private readonly Vector3 unitNegZ = new Vector3(0, 0, -1.0);

        /// <summary>
        /// Test context
        /// </summary>
        private TestContext testContextInstance;

        /// <summary>
        /// Initializes a new instance of the HomogenousTransformTest class
        /// </summary>
        public HomogenousTransformTest()
        {
        }

        /// <summary>
        /// Gets or sets the test context which provides
        /// information about and functionality for the current test run.
        /// </summary>
        public TestContext TestContext
        {
            get
            {
                return this.testContextInstance;
            }

            set
            {
                this.testContextInstance = value;
            }
        }

        /// <summary>
        /// Create a random homogenous transform.   Positional elements have a value between
        /// -1 and 1, rotation is based off of a random unit quaternion
        /// </summary>
        /// <returns>A Matrix4 containing random homogenous transform.</returns>
        public static Matrix4 GetRandomHomogenousTransform()
        {
            // create a random axis angle for the rotational component
            Random randGen = new Random();

            Quaternion quaternion = Quaternion.Normalize(
                new Quaternion(
                    randGen.NextDouble(),
                    randGen.NextDouble(),
                    randGen.NextDouble(),
                    randGen.NextDouble()));

            Matrix4 ht = HomogenousTransform.CreateFromQuaternion(quaternion);

            ht[0, 3] = 2.0 * randGen.NextDouble() - 1.0;
            ht[1, 3] = 2.0 * randGen.NextDouble() - 1.0;
            ht[2, 3] = 2.0 * randGen.NextDouble() - 1.0;

            return ht;
        }

        /// <summary>
        /// Test 0 angle rotation around an axis angle
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestCreationFromZeroRotationAxisAngle()
        {
            AxisAngle zeroRotationAxisAngle = new AxisAngle(new Vector3(0, 0, 1.0), 0);
            Matrix4 ht = HomogenousTransform.CreateFromAxisAngle(zeroRotationAxisAngle);
            this.AssertIsIdentityMatrix(ht);
        }

        /// <summary>
        /// Test 0 angle rotation around x-axis
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestCreationFromZeroXRotation()
        {
            Matrix4 ht = HomogenousTransform.CreateFromRotationX(0);
            this.AssertIsIdentityMatrix(ht);
        }

        /// <summary>
        /// Test 0 angle rotation around y-axis
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestCreationFromZeroYRotation()
        {
            Matrix4 ht = HomogenousTransform.CreateFromRotationX(0);
            this.AssertIsIdentityMatrix(ht);
        }

        /// <summary>
        /// Test 0 angle rotation around z-axis
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestCreationFromZeroZRotation()
        {
            Matrix4 ht = HomogenousTransform.CreateFromRotationX(0);
            this.AssertIsIdentityMatrix(ht);
        }

        /// <summary>
        /// Tests rotation matrices generated by HomogenousTransform.CreateFromRotationX by
        /// creating transforms for positive and negative rotations in cardinal directions
        /// and applying them to positive unit basis vectors
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestUnitAxisRotationsAroundXCardinalDirections()
        {
            Matrix4 homogenousTransformPos90 = HomogenousTransform.CreateFromRotationX(Math.PI / 2.0);
            Matrix4 homogenousTransformPos180 = HomogenousTransform.CreateFromRotationX(Math.PI);
            Matrix4 homogenousTransformPos270 = HomogenousTransform.CreateFromRotationX((3.0 * Math.PI) / 2.0);
            Matrix4 homogenousTransformPos360 = HomogenousTransform.CreateFromRotationX(2.0 * Math.PI);
            Matrix4 homogenousTransformNeg90 = HomogenousTransform.CreateFromRotationX(-Math.PI / 2.0);
            Matrix4 homogenousTransformNeg180 = HomogenousTransform.CreateFromRotationX(-Math.PI);
            Matrix4 homogenousTransformNeg270 = HomogenousTransform.CreateFromRotationX(-(3.0 * Math.PI) / 2.0);
            Matrix4 homogenousTransformNeg360 = HomogenousTransform.CreateFromRotationX(-2.0 * Math.PI);

            // Test rotations of X axis
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformPos90, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformPos180, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformPos270, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformPos360, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformNeg90, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformNeg180, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformNeg270, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformNeg360, this.unitPosX));

            // Test rotations of Y axis
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformPos90, this.unitPosY));
            Assert.AreEqual(this.unitNegY, HomogenousTransform.Apply(homogenousTransformPos180, this.unitPosY));
            Assert.AreEqual(this.unitNegZ, HomogenousTransform.Apply(homogenousTransformPos270, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformPos360, this.unitPosY));
            Assert.AreEqual(this.unitNegZ, HomogenousTransform.Apply(homogenousTransformNeg90, this.unitPosY));
            Assert.AreEqual(this.unitNegY, HomogenousTransform.Apply(homogenousTransformNeg180, this.unitPosY));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformNeg270, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformNeg360, this.unitPosY));

            // Test rotations of Z axis
            Assert.AreEqual(this.unitNegY, HomogenousTransform.Apply(homogenousTransformPos90, this.unitPosZ));
            Assert.AreEqual(this.unitNegZ, HomogenousTransform.Apply(homogenousTransformPos180, this.unitPosZ));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformPos270, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformPos360, this.unitPosZ));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformNeg90, this.unitPosZ));
            Assert.AreEqual(this.unitNegZ, HomogenousTransform.Apply(homogenousTransformNeg180, this.unitPosZ));
            Assert.AreEqual(this.unitNegY, HomogenousTransform.Apply(homogenousTransformNeg270, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformNeg360, this.unitPosZ));
        }

        /// <summary>
        /// Tests rotation matrices generated by HomogenousTransform.CreateFromRotationY by
        /// creating transforms for positive and negative rotations in cardinal directions
        /// and applying them to positive unit basis vectors
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestUnitAxisRotationsAroundYCardinalDirections()
        {
            Matrix4 homogenousTransformPos90 = HomogenousTransform.CreateFromRotationY(Math.PI / 2.0);
            Matrix4 homogenousTransformPos180 = HomogenousTransform.CreateFromRotationY(Math.PI);
            Matrix4 homogenousTransformPos270 = HomogenousTransform.CreateFromRotationY((3.0 * Math.PI) / 2.0);
            Matrix4 homogenousTransformPos360 = HomogenousTransform.CreateFromRotationY(2.0 * Math.PI);
            Matrix4 homogenousTransformNeg90 = HomogenousTransform.CreateFromRotationY(-Math.PI / 2.0);
            Matrix4 homogenousTransformNeg180 = HomogenousTransform.CreateFromRotationY(-Math.PI);
            Matrix4 homogenousTransformNeg270 = HomogenousTransform.CreateFromRotationY(-(3.0 * Math.PI) / 2.0);
            Matrix4 homogenousTransformNeg360 = HomogenousTransform.CreateFromRotationY(-2.0 * Math.PI);

            // Test rotations of X axis
            Assert.AreEqual(this.unitNegZ, HomogenousTransform.Apply(homogenousTransformPos90, this.unitPosX));
            Assert.AreEqual(this.unitNegX, HomogenousTransform.Apply(homogenousTransformPos180, this.unitPosX));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformPos270, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformPos360, this.unitPosX));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformNeg90, this.unitPosX));
            Assert.AreEqual(this.unitNegX, HomogenousTransform.Apply(homogenousTransformNeg180, this.unitPosX));
            Assert.AreEqual(this.unitNegZ, HomogenousTransform.Apply(homogenousTransformNeg270, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformNeg360, this.unitPosX));

            // Test rotations of Y axis
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformPos90, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformPos180, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformPos270, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformPos360, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformNeg90, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformNeg180, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformNeg270, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformNeg360, this.unitPosY));

            // Test rotations of Z axis
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformPos90, this.unitPosZ));
            Assert.AreEqual(this.unitNegZ, HomogenousTransform.Apply(homogenousTransformPos180, this.unitPosZ));
            Assert.AreEqual(this.unitNegX, HomogenousTransform.Apply(homogenousTransformPos270, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformPos360, this.unitPosZ));
            Assert.AreEqual(this.unitNegX, HomogenousTransform.Apply(homogenousTransformNeg90, this.unitPosZ));
            Assert.AreEqual(this.unitNegZ, HomogenousTransform.Apply(homogenousTransformNeg180, this.unitPosZ));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformNeg270, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformNeg360, this.unitPosZ));
        }

        /// <summary>
        /// Tests rotation matrices generated by HomogenousTransform.CreateFromRotationZ by
        /// creating transforms for positive and negative rotations in cardinal directions
        /// and applying them to positive unit basis vectors
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestUnitAxisRotationsAroundZCardinalDirections()
        {
            Matrix4 homogenousTransformPos90 = HomogenousTransform.CreateFromRotationZ(Math.PI / 2.0);
            Matrix4 homogenousTransformPos180 = HomogenousTransform.CreateFromRotationZ(Math.PI);
            Matrix4 homogenousTransformPos270 = HomogenousTransform.CreateFromRotationZ((3.0 * Math.PI) / 2.0);
            Matrix4 homogenousTransformPos360 = HomogenousTransform.CreateFromRotationZ(2.0 * Math.PI);
            Matrix4 homogenousTransformNeg90 = HomogenousTransform.CreateFromRotationZ(-Math.PI / 2.0);
            Matrix4 homogenousTransformNeg180 = HomogenousTransform.CreateFromRotationZ(-Math.PI);
            Matrix4 homogenousTransformNeg270 = HomogenousTransform.CreateFromRotationZ(-(3.0 * Math.PI) / 2.0);
            Matrix4 homogenousTransformNeg360 = HomogenousTransform.CreateFromRotationZ(-2.0 * Math.PI);

            // Test rotations of X axis
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformPos90, this.unitPosX));
            Assert.AreEqual(this.unitNegX, HomogenousTransform.Apply(homogenousTransformPos180, this.unitPosX));
            Assert.AreEqual(this.unitNegY, HomogenousTransform.Apply(homogenousTransformPos270, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformPos360, this.unitPosX));
            Assert.AreEqual(this.unitNegY, HomogenousTransform.Apply(homogenousTransformNeg90, this.unitPosX));
            Assert.AreEqual(this.unitNegX, HomogenousTransform.Apply(homogenousTransformNeg180, this.unitPosX));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformNeg270, this.unitPosX));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformNeg360, this.unitPosX));

            // Test rotations of Y axis
            Assert.AreEqual(this.unitNegX, HomogenousTransform.Apply(homogenousTransformPos90, this.unitPosY));
            Assert.AreEqual(this.unitNegY, HomogenousTransform.Apply(homogenousTransformPos180, this.unitPosY));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformPos270, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformPos360, this.unitPosY));
            Assert.AreEqual(this.unitPosX, HomogenousTransform.Apply(homogenousTransformNeg90, this.unitPosY));
            Assert.AreEqual(this.unitNegY, HomogenousTransform.Apply(homogenousTransformNeg180, this.unitPosY));
            Assert.AreEqual(this.unitNegX, HomogenousTransform.Apply(homogenousTransformNeg270, this.unitPosY));
            Assert.AreEqual(this.unitPosY, HomogenousTransform.Apply(homogenousTransformNeg360, this.unitPosY));

            // Test rotations of Z axis
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformPos90, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformPos180, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformPos270, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformPos360, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformNeg90, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformNeg180, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformNeg270, this.unitPosZ));
            Assert.AreEqual(this.unitPosZ, HomogenousTransform.Apply(homogenousTransformNeg360, this.unitPosZ));
        }

        /// <summary>
        /// Test the creation of homogenous transforms from axis angles pointing in cardinal
        /// directions.  Will be tested by comparing to results  of CreateFromRotationX, 
        /// CreateFromRotationY, and CreateFromRotationZ which are tested against ground 
        /// truth
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestAxisAngleCardinalDirections()
        {
            // Test rotations around X axis
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationX(Math.PI / 2.0),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(1.0, 0.0, 0.0), Math.PI / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationX(Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(1.0, 0.0, 0.0), Math.PI)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationX((3.0 * Math.PI) / 2.0),
                HomogenousTransform.CreateFromAxisAngle(
                    new AxisAngle(new Vector3(1.0, 0.0, 0.0), (3.0 * Math.PI) / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationX(2.0 * Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(1.0, 0.0, 0.0), 2.0 * Math.PI)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationX(-Math.PI / 2.0),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(1.0, 0.0, 0.0), -Math.PI / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationX(-Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(1.0, 0.0, 0.0), -Math.PI)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationX(-(3.0 * Math.PI) / 2.0),
                HomogenousTransform.CreateFromAxisAngle(
                    new AxisAngle(new Vector3(1.0, 0.0, 0.0), -(3.0 * Math.PI) / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationX(-2.0 * Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(1.0, 0.0, 0.0), -2.0 * Math.PI)));

            // Test rotations around Y axis
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationY(Math.PI / 2.0),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 1.0, 0.0), Math.PI / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationY(Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 1.0, 0.0), Math.PI)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationY((3.0 * Math.PI) / 2.0),
                HomogenousTransform.CreateFromAxisAngle(
                    new AxisAngle(new Vector3(0.0, 1.0, 0.0), (3.0 * Math.PI) / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationY(2.0 * Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 1.0, 0.0), 2.0 * Math.PI)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationY(-Math.PI / 2.0),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 1.0, 0.0), -Math.PI / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationY(-Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 1.0, 0.0), -Math.PI)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationY(-(3.0 * Math.PI) / 2.0),
                HomogenousTransform.CreateFromAxisAngle(
                    new AxisAngle(new Vector3(0.0, 1.0, 0.0), -(3.0 * Math.PI) / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationY(-2.0 * Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 1.0, 0.0), -2.0 * Math.PI)));

            // Test rotations around Z axis
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationZ(Math.PI / 2.0),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 0.0, 1.0), Math.PI / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationZ(Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 0.0, 1.0), Math.PI)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationZ((3.0 * Math.PI) / 2.0),
                HomogenousTransform.CreateFromAxisAngle(
                    new AxisAngle(new Vector3(0.0, 0.0, 1.0), (3.0 * Math.PI) / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationZ(2.0 * Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 0.0, 1.0), 2.0 * Math.PI)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationZ(-Math.PI / 2.0),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 0.0, 1.0), -Math.PI / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationZ(-Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 0.0, 1.0), -Math.PI)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationZ(-(3.0 * Math.PI) / 2.0),
                HomogenousTransform.CreateFromAxisAngle(
                    new AxisAngle(new Vector3(0.0, 0.0, 1.0), -(3.0 * Math.PI) / 2.0)));
            this.AssertElementsAreEqual(
                HomogenousTransform.CreateFromRotationZ(-2.0 * Math.PI),
                HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0.0, 0.0, 1.0), -2.0 * Math.PI)));
        }

        /// <summary> 
        /// Create a quaternion from an Axis Angle and confirm that 
        /// both produce the same Homogenous Transform using appropriate
        /// methods.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestQuaterionRotationAgainstAxisAngle()
        {
            AxisAngle aa = new AxisAngle(
                new Vector3(Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0)), Math.PI / 4.0);

            Quaternion q = Quaternion.FromAxisAngle(aa);

            Matrix4 axisAngleHt = HomogenousTransform.CreateFromAxisAngle(aa);
            Matrix4 quaternionHt = HomogenousTransform.CreateFromQuaternion(q);

            this.AssertElementsAreEqual(axisAngleHt, quaternionHt);
        }

        /// <summary> 
        /// Test creating a homogeneous matrix from rotations around ZYX.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestRotationAroundZYX()
        {
            double z = Math.PI;
            double y = MathConstants.PIOverTwo;
            double x = MathConstants.PIOverFour;
            Matrix4 testMatrix = HomogenousTransform.CreateFromEulerAngleZYX(z, y, x);

            Matrix4 rotz = HomogenousTransform.CreateFromRotationZ(z);
            Matrix4 roty = HomogenousTransform.CreateFromRotationY(y);
            Matrix4 rotx = HomogenousTransform.CreateFromRotationX(x);

            this.AssertElementsAreEqual(testMatrix, rotz * roty * rotx);
        }

        /// <summary>
        /// Verifying implementation of Rodrigues function against cardinal directions for rotation matrices
        /// which are verified elsewhere
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestInternalRodriguessImplementation()
        {
            double[] testAngles = new[]
                {
                    Math.PI / 2.0, Math.PI, (3.0 * Math.PI) / 2.0, 2.0 * Math.PI, -Math.PI / 2.0, -Math.PI, 
                    -(3.0 * Math.PI) / 2.0, -2.0 * Math.PI
                };

            Vector3 testPoint = new Vector3(1.0, 1.0, 1.0);

            foreach (double angle in testAngles)
            {
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationX(angle), testPoint),
                    this.ComputeRotationUsingRodriguessFormula(
                        new AxisAngle(new Vector3(1.0, 0.0, 0.0), angle), testPoint));
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationY(angle), testPoint),
                    this.ComputeRotationUsingRodriguessFormula(
                        new AxisAngle(new Vector3(0.0, 1.0, 0.0), angle), testPoint));
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationZ(angle), testPoint),
                    this.ComputeRotationUsingRodriguessFormula(
                        new AxisAngle(new Vector3(0.0, 0.0, 1.0), angle), testPoint));
            }
        }

        /// <summary>
        /// Test that implementation of axis angle rotations give same results as Rodrigues's
        /// formula for 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestAxisAngleAndRodriguessGiveSameResult()
        {
            // use non-cardinal directions
            double[] testAngles = new[]
                {
                    (Math.PI / 2.0) + 0.1, Math.PI + 0.2, ((3.0 * Math.PI) / 2.0) + 0.3, (2.0 * Math.PI) + 0.4, 
                    (-Math.PI / 2.0) + 0.5, -Math.PI + 0.6, (-(3.0 * Math.PI) / 2.0) + 0.7, (-2.0 * Math.PI) + 0.8
                };

            Vector3 testPoint = new Vector3(1.0, 2.0, 3.0);
            Vector3 axisOfRotation = new Vector3(Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0));

            foreach (double angle in testAngles)
            {
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromAxisAngle(new AxisAngle(axisOfRotation, angle)), testPoint),
                    this.ComputeRotationUsingRodriguessFormula(new AxisAngle(axisOfRotation, angle), testPoint));
            }
        }

        /// <summary>
        /// Use internal implementation of Rodrigues's method to test the implementation of quaternion based rotations.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestQuaternionAndRodriguessGiveSameResult()
        {
            // use non-cardinal directions
            double[] testAngles = new[]
                {
                    (Math.PI / 2.0) + 0.15, Math.PI + 0.25, ((3.0 * Math.PI) / 2.0) + 0.35, (2.0 * Math.PI) + 0.45, 
                    (-Math.PI / 2.0) + 0.55, -Math.PI + 0.65, (-(3.0 * Math.PI) / 2.0) + 0.75, (-2.0 * Math.PI) + 0.85
                };

            Vector3 testPoint = new Vector3(1.0, 2.0, 3.0);
            Vector3 axisOfRotation = new Vector3(Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0));

            foreach (double angle in testAngles)
            {
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromQuaternion(
                            Quaternion.FromAxisAngle(new AxisAngle(axisOfRotation, angle))),
                        testPoint),
                    this.ComputeRotationUsingRodriguessFormula(new AxisAngle(axisOfRotation, angle), testPoint));
            }
        }

        /// <summary>
        /// Test that a transform created from a pose works as expected
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestGeneralTransformFromPose()
        {
            Vector3 position = new Vector3(4.0, 5.0, 6.0);
            Vector3 rotationAxis = new Vector3(Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0));
            double rotationAngle = Math.PI / 2.0;
            Quaternion orientation = Quaternion.FromAxisAngle(new AxisAngle(rotationAxis, rotationAngle));
            Pose pose = new Pose(position, orientation);
            Matrix4 ht = HomogenousTransform.CreateFromPose(pose);

            // confirm that a point located at the origin of the frame defined by the pose is in the correct place
            this.AssertElementsAreEqual(position, HomogenousTransform.Apply(ht, new Vector3(0.0, 0.0, 0.0)));

            // [1,1,1] in the frame defined by the pose should be equal to [5, 6, 7] in base frame
            this.AssertElementsAreEqual(
                new Vector3(5.0, 6.0, 7.0), HomogenousTransform.Apply(ht, new Vector3(1.0, 1.0, 1.0)));
        }

        /// <summary>
        /// Test that a pose with only a rotational component behaves properly
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestRotationsFromPose()
        {
            Vector3 origin = new Vector3(0.0, 0.0, 0.0);

            // will use cardinal rotations
            double[] testAngles = new[]
                {
                    Math.PI / 2.0, Math.PI, (3.0 * Math.PI) / 2.0, 2.0 * Math.PI, -Math.PI / 2.0, -Math.PI, 
                    -(3.0 * Math.PI) / 2.0, -2.0 * Math.PI
                };

            // will test along principle axes
            foreach (double angle in testAngles)
            {
                // rotations around x
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationX(angle), this.unitPosX),
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromPose(
                            new Pose(origin, Quaternion.FromAxisAngle(new AxisAngle(this.unitPosX, angle)))),
                        this.unitPosX));
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationX(angle), this.unitPosY),
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromPose(
                            new Pose(origin, Quaternion.FromAxisAngle(new AxisAngle(this.unitPosX, angle)))),
                        this.unitPosY));
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationX(angle), this.unitPosZ),
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromPose(
                            new Pose(origin, Quaternion.FromAxisAngle(new AxisAngle(this.unitPosX, angle)))),
                        this.unitPosZ));

                // rotations around y
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationY(angle), this.unitPosX),
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromPose(
                            new Pose(origin, Quaternion.FromAxisAngle(new AxisAngle(this.unitPosY, angle)))),
                        this.unitPosX));
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationY(angle), this.unitPosY),
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromPose(
                            new Pose(origin, Quaternion.FromAxisAngle(new AxisAngle(this.unitPosY, angle)))),
                        this.unitPosY));
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationY(angle), this.unitPosZ),
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromPose(
                            new Pose(origin, Quaternion.FromAxisAngle(new AxisAngle(this.unitPosY, angle)))),
                        this.unitPosZ));

                // rotations around z
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationZ(angle), this.unitPosX),
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromPose(
                            new Pose(origin, Quaternion.FromAxisAngle(new AxisAngle(this.unitPosZ, angle)))),
                        this.unitPosX));
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationZ(angle), this.unitPosY),
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromPose(
                            new Pose(origin, Quaternion.FromAxisAngle(new AxisAngle(this.unitPosZ, angle)))),
                        this.unitPosY));
                this.AssertElementsAreEqual(
                    HomogenousTransform.Apply(HomogenousTransform.CreateFromRotationZ(angle), this.unitPosZ),
                    HomogenousTransform.Apply(
                        HomogenousTransform.CreateFromPose(
                            new Pose(origin, Quaternion.FromAxisAngle(new AxisAngle(this.unitPosZ, angle)))),
                        this.unitPosZ));
            }
        }

        /// <summary>
        /// Test inversion of homogenous transforms
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestInversion()
        {
            // create a transform from a pose
            Vector3 position = new Vector3(4.0, 5.0, 6.0);
            Vector3 rotationAxis = new Vector3(Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0));
            double rotationAngle = Math.PI / 2.0;
            Quaternion orientation = Quaternion.FromAxisAngle(new AxisAngle(rotationAxis, rotationAngle));
            Pose pose = new Pose(position, orientation);
            Matrix4 ht = HomogenousTransform.CreateFromPose(pose);

            // inversion should give same result as matrix inversion
            this.AssertElementsAreEqual(HomogenousTransform.Inverse(ht), Matrix4.Inverse(ht));
        }

        /// <summary>
        /// Test multiplication of homogenous transforms
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestMulitplication()
        {
            // create a transform from a pose
            Vector3 position1 = new Vector3(4.0, 5.0, 6.0);
            Vector3 rotationAxis1 = new Vector3(Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0), Math.Sqrt(1.0 / 3.0));
            double rotationAngle1 = Math.PI / 2.0;
            Quaternion orientation1 = Quaternion.FromAxisAngle(new AxisAngle(rotationAxis1, rotationAngle1));
            Pose pose1 = new Pose(position1, orientation1);
            Matrix4 ht1 = HomogenousTransform.CreateFromPose(pose1);

            // create a second transform from a pose
            Vector3 position2 = new Vector3(2.0, 4.0, 8.0);
            Vector3 rotationAxis2 = new Vector3(Math.Sqrt(1.0 / 2.0), Math.Sqrt(1.0 / 2.0), 0);
            double rotationAngle2 = 1.0;
            Quaternion orientation2 = Quaternion.FromAxisAngle(new AxisAngle(rotationAxis2, rotationAngle2));
            Pose pose2 = new Pose(position2, orientation2);
            Matrix4 ht2 = HomogenousTransform.CreateFromPose(pose2);

            // multiplication should give same result as matrix multiplication
            this.AssertElementsAreEqual(HomogenousTransform.Multiply(ht1, ht2), ht1 * ht2);
        }

        /// <summary>
        /// Test multiplication of homogenous transforms
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestConvertToFrame()
        {
            Matrix4 transform = GetRandomHomogenousTransform();

            double[] frame = HomogenousTransform.ToFrameArray(transform);

            Assert.AreEqual(HomogenousTransform.FrameArrayLength, frame.Length, "Frame length is correct");

            Assert.AreEqual(transform[0, 0], frame[0], "Frame values are correct");
            Assert.AreEqual(transform[0, 1], frame[1], "Frame values are correct");
            Assert.AreEqual(transform[0, 2], frame[2], "Frame values are correct");
            Assert.AreEqual(transform[0, 3], frame[3], "Frame values are correct");
            Assert.AreEqual(transform[1, 0], frame[4], "Frame values are correct");
            Assert.AreEqual(transform[1, 1], frame[5], "Frame values are correct");
            Assert.AreEqual(transform[1, 2], frame[6], "Frame values are correct");
            Assert.AreEqual(transform[1, 3], frame[7], "Frame values are correct");
            Assert.AreEqual(transform[2, 0], frame[8], "Frame values are correct");
            Assert.AreEqual(transform[2, 1], frame[9], "Frame values are correct");
            Assert.AreEqual(transform[2, 2], frame[10], "Frame values are correct");
            Assert.AreEqual(transform[2, 3], frame[11], "Frame values are correct");

            Matrix4 reconvertedTransform = HomogenousTransform.FromFrameArray(frame);

            Assert.AreEqual(transform, reconvertedTransform, "Transform is same after converting to frame array and back");
        }

        /// <summary>
        /// Test conversion from a homogeneous transform to an AxisAngle
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestConvertToAxisAngle()
        {
            double sqrt2Over2 = Math.Sqrt(2.0) / 2;
            Matrix4 noRotation = new Matrix4(new double[4, 4] { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } });

            Matrix4 rot180AroundZ = new Matrix4(new double[4, 4] { { -1, 0, 0, 0 }, { 0, -1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } });
            Matrix4 rot180AroundY = new Matrix4(new double[4, 4] { { -1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, -1, 0 }, { 0, 0, 0, 1 } });
            Matrix4 rot180AroundX = new Matrix4(new double[4, 4] { { 1, 0, 0, 0 }, { 0, -1, 0, 0 }, { 0, 0, -1, 0 }, { 0, 0, 0, 1 } });

            Matrix4 rot45AroundZ = new Matrix4(new double[4, 4] { { sqrt2Over2, -sqrt2Over2, 0, 0 }, { sqrt2Over2, sqrt2Over2, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } });

            // rotation around axis -Z [0,0,-1]
            Matrix4 rot45AroundNZ = new Matrix4(new double[4, 4] { { sqrt2Over2, sqrt2Over2, 0, 0 }, { -sqrt2Over2, sqrt2Over2, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } });

            // rotation around axis YZ [0,1,1]
            Matrix4 rot180AroundYZ = new Matrix4(new double[4, 4] { { -1, 0, 0, 0 }, { 0, 0, 1, 0 }, { 0, 1, 0, 0 }, { 0, 0, 0, 1 } });

            // rotation around axis XZ [1,0,1]
            Matrix4 rot180AroundXZ = new Matrix4(new double[4, 4] { { 0, 0, 1, 0 }, { 0, -1, 0, 0 }, { 1, 0, 0, 0 }, { 0, 0, 0, 1 } });

            // rotation around axis XY [1,1,0]
            Matrix4 rot180AroundXY = new Matrix4(new double[4, 4] { { 0, 1, 0, 0 }, { 1, 0, 0, 0 }, { 0, 0, -1, 0 }, { 0, 0, 0, 1 } });

            Matrix4 rot90AroundY = new Matrix4(new double[4, 4] { { 0, 0, 1, 0 }, { 0, 1, 0, 0 }, { -1, 0, 0, 0 }, { 0, 0, 0, 1 } });

            // rotation around axis -Y [0,-1,0]
            Matrix4 rot90AroundNY = new Matrix4(new double[4, 4] { { 0, 0, -1, 0 }, { 0, 1, 0, 0 }, { 1, 0, 0, 0 }, { 0, 0, 0, 1 } });

            // rotation around axis XYZ [1,1,1]
            Matrix4 rot120AroundXYZ = new Matrix4(new double[4, 4] { { 0, 0, 1, 0 }, { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 0, 1 } });

            AxisAngle test1 = HomogenousTransform.ToAxisAngle(noRotation);
            Assert.AreEqual(test1.Angle, 0);

            // 180 degree rotaiton around Z
            AxisAngle test2 = HomogenousTransform.ToAxisAngle(rot180AroundZ);
            Assert.AreEqual(test2.Angle, Math.PI);
            Assert.AreEqual(test2.Axis.X, 0);
            Assert.AreEqual(test2.Axis.Y, 0);
            Assert.AreEqual(test2.Axis.Z, 1);

            // 180 degree rotaiton around Y
            AxisAngle test3 = HomogenousTransform.ToAxisAngle(rot180AroundY);
            Assert.AreEqual(test3.Angle, Math.PI);
            Assert.AreEqual(test3.Axis.X, 0);
            Assert.AreEqual(test3.Axis.Y, 1);
            Assert.AreEqual(test3.Axis.Z, 0);

            // 180 degree rotaiton around X
            AxisAngle test4 = HomogenousTransform.ToAxisAngle(rot180AroundX);
            Assert.AreEqual(test4.Angle, Math.PI);
            Assert.AreEqual(test4.Axis.X, 1);
            Assert.AreEqual(test4.Axis.Y, 0);
            Assert.AreEqual(test4.Axis.Z, 0);

            // 45 degree rotaiton around Z
            AxisAngle test5 = HomogenousTransform.ToAxisAngle(rot45AroundZ);
            this.AssertDoubleEqualWithNanTest(test5.Angle, MathConstants.PIOverFour, MathConstants.ErrorEpsilon);
            Assert.AreEqual(test5.Axis.X, 0);
            Assert.AreEqual(test5.Axis.Y, 0);
            Assert.AreEqual(test5.Axis.Z, 1);

            // 45 degree rotaiton around -Z
            AxisAngle test6 = HomogenousTransform.ToAxisAngle(rot45AroundNZ);
            this.AssertDoubleEqualWithNanTest(test6.Angle, MathConstants.PIOverFour, MathConstants.ErrorEpsilon);
            Assert.AreEqual(test6.Axis.X, 0);
            Assert.AreEqual(test6.Axis.Y, 0);
            Assert.AreEqual(test6.Axis.Z, -1);

            // 180 degree rotaiton around YZ [0,1,1]
            AxisAngle test7 = HomogenousTransform.ToAxisAngle(rot180AroundYZ);
            Matrix4 m = HomogenousTransform.CreateFromAxisAngle(test7);
            Assert.AreEqual(test7.Angle, Math.PI);
            Assert.AreEqual(test7.Axis.X, 0);
            this.AssertDoubleEqualWithNanTest(test7.Axis.Y, sqrt2Over2, MathConstants.ErrorEpsilon);
            this.AssertDoubleEqualWithNanTest(test7.Axis.Z, sqrt2Over2, MathConstants.ErrorEpsilon);

            // 180 degree rotaiton around XZ [1,0,1]
            AxisAngle test8 = HomogenousTransform.ToAxisAngle(rot180AroundXZ);
            Assert.AreEqual(test8.Angle, Math.PI);
            this.AssertDoubleEqualWithNanTest(test8.Axis.X, sqrt2Over2, MathConstants.ErrorEpsilon);
            Assert.AreEqual(test8.Axis.Y, 0);
            this.AssertDoubleEqualWithNanTest(test8.Axis.Z, sqrt2Over2, MathConstants.ErrorEpsilon);

            // 180 degree rotaiton around XY [1,1,0]
            AxisAngle test9 = HomogenousTransform.ToAxisAngle(rot180AroundXY);
            Assert.AreEqual(test9.Angle, Math.PI);
            this.AssertDoubleEqualWithNanTest(test9.Axis.X, sqrt2Over2, MathConstants.ErrorEpsilon);
            this.AssertDoubleEqualWithNanTest(test9.Axis.Y, sqrt2Over2, MathConstants.ErrorEpsilon);
            Assert.AreEqual(test9.Axis.Z, 0);

            // 90 degree rotaiton around Y
            AxisAngle test10 = HomogenousTransform.ToAxisAngle(rot90AroundY);
            this.AssertDoubleEqualWithNanTest(test10.Angle, MathConstants.PIOverTwo, MathConstants.ErrorEpsilon);
            Assert.AreEqual(test10.Axis.X, 0);
            Assert.AreEqual(test10.Axis.Y, 1);
            Assert.AreEqual(test10.Axis.Z, 0);

            // 90 degree rotaiton around -Y
            AxisAngle test11 = HomogenousTransform.ToAxisAngle(rot90AroundNY);
            this.AssertDoubleEqualWithNanTest(test11.Angle, MathConstants.PIOverTwo, MathConstants.ErrorEpsilon);
            Assert.AreEqual(test11.Axis.X, 0);
            Assert.AreEqual(test11.Axis.Y, -1);
            Assert.AreEqual(test11.Axis.Z, 0);

            // 120 degree rotaiton around XYZ [1,1,1]
            AxisAngle test12 = HomogenousTransform.ToAxisAngle(rot120AroundXYZ);
            this.AssertDoubleEqualWithNanTest(test12.Angle, Math.PI * 2 / 3, MathConstants.ErrorEpsilon);
            this.AssertDoubleEqualWithNanTest(test12.Axis.X, Math.Sqrt(3) / 3, MathConstants.ErrorEpsilon);
            this.AssertDoubleEqualWithNanTest(test12.Axis.Y, Math.Sqrt(3) / 3, MathConstants.ErrorEpsilon);
            this.AssertDoubleEqualWithNanTest(test12.Axis.Z, Math.Sqrt(3) / 3, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test making a rotation matrix orthogonal
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestOrthogonalizeRotationMatrix()
        {
            // test 1
            Matrix4 transf = new Matrix4(new double[,] { { 0.5, 0, 0, 0 }, { 0, 0.5, 0, 0 }, { 0, 0, 0.5, 0 }, { 0, 0, 0, 1 } });
            HomogenousTransform.OrthogonalizeRotationalMatrix(transf);
            this.AssertIsIdentityMatrix(transf);

            // test 2
            double squreRootOfTwo = Math.Sqrt(2.0);
            transf[0, 0] = squreRootOfTwo / 2.1;
            transf[1, 0] = squreRootOfTwo / 2.1;
            transf[0, 1] = squreRootOfTwo / 2.3;
            transf[1, 1] = -squreRootOfTwo / 2.3;
            transf[2, 2] = -0.9;
            HomogenousTransform.OrthogonalizeRotationalMatrix(transf);
            Assert.AreEqual(transf[0, 0], squreRootOfTwo / 2.0, MathConstants.ErrorEpsilon);
            Assert.AreEqual(transf[1, 0], squreRootOfTwo / 2.0, MathConstants.ErrorEpsilon);
            Assert.AreEqual(transf[0, 1], squreRootOfTwo / 2.0, MathConstants.ErrorEpsilon);
            Assert.AreEqual(transf[1, 1], -squreRootOfTwo / 2.0, MathConstants.ErrorEpsilon);
            Assert.AreEqual(transf[2, 2], -1, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test creating the transformation from two vectors
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CreateFromTwoVectorsTest()
        {
            int seed = 425722734;
            int iterations = 10000;

            // test a large numbe of random vectors
            Random random = new Random(seed);
            for (int i = 0; i < iterations; ++i)
            {
                Vector3 vecA = this.RandomVector3(random);
                Vector3 vecB = this.RandomVector3(random);

                Matrix4 transform = HomogenousTransform.CreateFromTwoVectors(vecA, vecB);

                Vector3 vecATransformed = HomogenousTransform.Apply(transform, vecA);

                vecATransformed = Vector3.Normalize(vecATransformed);
                vecB = Vector3.Normalize(vecB);
                Assert.AreEqual(vecATransformed, vecB);
            }

            // test that we get the identity matrix if we give it the same vector
            Matrix4 identityTransform = HomogenousTransform.CreateFromTwoVectors(Vector3.XAxis, Vector3.XAxis);

            for (int i = 0; i < 4; ++i)
            {
                for (int j = 0; j < 4; ++j)
                {
                    if (i == j)
                    {
                        Assert.AreEqual(identityTransform[i, j], 1.0, MathConstants.ErrorEpsilon);
                    }
                    else
                    {
                        Assert.AreEqual(identityTransform[i, j], 0, MathConstants.ErrorEpsilon);
                    }
                }
            }
        }

        /// <summary>
        /// Asserts that the given matrix is the identity matrix
        /// </summary>
        /// <param name="matrix">A matrix</param>
        private void AssertIsIdentityMatrix(Matrix4 matrix)
        {
            Assert.AreEqual(matrix[0, 0], 1.0);
            Assert.AreEqual(matrix[1, 1], 1.0);
            Assert.AreEqual(matrix[2, 2], 1.0);
            Assert.AreEqual(matrix[3, 3], 1.0);
            Assert.AreEqual(matrix[0, 1], 0.0);
            Assert.AreEqual(matrix[0, 2], 0.0);
            Assert.AreEqual(matrix[0, 3], 0.0);
            Assert.AreEqual(matrix[1, 0], 0.0);
            Assert.AreEqual(matrix[1, 2], 0.0);
            Assert.AreEqual(matrix[1, 3], 0.0);
            Assert.AreEqual(matrix[2, 0], 0.0);
            Assert.AreEqual(matrix[2, 1], 0.0);
            Assert.AreEqual(matrix[2, 3], 0.0);
            Assert.AreEqual(matrix[3, 0], 0.0);
            Assert.AreEqual(matrix[3, 1], 0.0);
            Assert.AreEqual(matrix[3, 2], 0.0);
        }

        /// <summary>
        /// Asserts that the elements of the given matrix are equal
        /// </summary>
        /// <param name="m1">A matrix</param>
        /// <param name="m2">Another matrix</param>
        private void AssertElementsAreEqual(Matrix4 m1, Matrix4 m2)
        {
            Assert.IsTrue(Math.Abs(m1[0, 0] - m2[0, 0]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[0, 1] - m2[0, 1]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[0, 2] - m2[0, 2]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[0, 3] - m2[0, 3]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[1, 0] - m2[1, 0]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[1, 1] - m2[1, 1]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[1, 2] - m2[1, 2]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[1, 3] - m2[1, 3]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[2, 0] - m2[2, 0]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[2, 1] - m2[2, 1]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[2, 2] - m2[2, 2]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[2, 3] - m2[2, 3]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[3, 0] - m2[3, 0]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[3, 1] - m2[3, 1]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[3, 2] - m2[3, 2]) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(m1[3, 3] - m2[3, 3]) < MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Asserts that the elements of the given vectors are equal
        /// </summary>
        /// <param name="v1">A vector</param>
        /// <param name="v2">Another vector</param>
        private void AssertElementsAreEqual(Vector3 v1, Vector3 v2)
        {
            Assert.IsTrue(Math.Abs(v1.X - v2.X) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(v1.Y - v2.Y) < MathConstants.ErrorEpsilon);
            Assert.IsTrue(Math.Abs(v1.Z - v2.Z) < MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Rotate a point (represented by a vector) around an axis angle using Roddrigues's formula.
        /// Used for checking other implementations.
        /// </summary>
        /// <param name="aa">The axis angle to rotate around</param>
        /// <param name="x">Vector representing the point to be rotated</param>>
        /// <returns>Rotated vector</returns>
        private Vector3 ComputeRotationUsingRodriguessFormula(AxisAngle aa, Vector3 x)
        {
            // Expression from Mechanics of Robotic Manipulation by M. Mason, 2001.  Equation (3.3)
            // Reversed sign on 'sin' term because we are in a left handed coordinate system.
            return x + (Math.Sin(aa.Angle) * Vector3.Cross(aa.Axis, x))
                   + ((1.0 - Math.Cos(aa.Angle)) * Vector3.Cross(aa.Axis, Vector3.Cross(aa.Axis, x)));
        }

        /// <summary>
        /// Test whether actual value is in the neighborhood of expected value.
        /// Use this test function instead of regular Assert.AreEqual if tolerance is set.
        /// Assert.AreEqual will pass when comparing an NaN with a real number.
        /// </summary>
        /// <param name="actual">The actual value</param>
        /// <param name="expected">The expected value</param>
        /// <param name="tolerance">The error tolerance</param>
        private void AssertDoubleEqualWithNanTest(double actual, double expected, double tolerance)
        {
            Assert.IsFalse(double.IsNaN(actual));
            Assert.IsFalse(double.IsNaN(expected));
            Assert.IsFalse(double.IsNaN(tolerance));
            Assert.AreEqual(expected, actual, tolerance);
        }

        /// <summary>
        /// Get a random Vector3
        /// </summary>
        /// <param name="random">Random number generator.</param>
        /// <returns>Random Vector3.</returns>
        private Vector3 RandomVector3(Random random)
        {
            return new Vector3(random.NextDouble(), random.NextDouble(), random.NextDouble());
        }
    }
}
